package nxt.WallRobo_03;
import lejos.nxt.*;
public class Correctionthread extends Thread
{
	public static Object wait_for_correction = new Object();
	int Correction = 0;
	public Usthread ust;
	public Correctionthread(){}
	public void run()
	{
		Sound.playTone(2000, 500);
		ust.correction_in_progress = true;

//Motor(B) 20° entgegen Drift setzen
		Motor.B.rotateTo((int)Math.round((20*43.328)*-(ust.Drift/Math.abs(ust.Drift))));
		Motor.B.stop();

		
//Correction
		
		//Correction=Radstand*2Pi*360*Betrag(Drift)/(sin(30)*2Pi*Radradius*360)

		Correction = (int) Math.round(Math.abs(ust.Drift)*8.25975);
		LCD.clear();
		LCD.drawInt(Correction, 0, 4);
		LCD.drawInt((int)ust.Drift, 0, 3);
		Motor.A.rotate(-Correction);

//Correction Beenden
		
		Motor.A.stop();
		Motor.B.stop();
		Motor.B.rotateTo(0);
		Motor.B.stop();
		
		ust.correction_is_needed = false;
		Sound.playTone(2000,100);
		try
		{
			synchronized(wait_for_correction)
			{	
				wait_for_correction.notifyAll();
			}
		}
		catch (Exception e){}
		ust.correction_in_progress = false;
		ust.Drift=0;
		Sound.playTone(400, 500);
		ust.last_distance_distance = 0;
		ust.last_distance_Tacho = 0;
		ust.current_distance_distance = 0;
		ust.current_distance_Tacho = 0;
	}
}